#include "opengl_3.h"

#include <GL/glew.h>
#include <GL/gl.h>
#include <string>
#include <vector>
#include <fstream>
#include <iostream>
#include "Models/legoMan/LegoMan.h"
using namespace std;
using namespace glm;

bool* pkeys;


/**
  Default constructor for the OpenGLContext class. At this stage it does nothing
  but you can put anything you want here.
*/
OpenGLContext::OpenGLContext(void) {

}

#ifndef _CAVE_
/**
  Constructor for the OpenGLContext class which will create a context given a windows HWND.
*/
OpenGLContext::OpenGLContext(HWND hwnd) {
  create30Context(hwnd); // Create a context given a HWND
}
#endif
/**
  Destructor for our OpenGLContext class which will clean up our rendering context
  and release the device context from the current window.
*/
OpenGLContext::~OpenGLContext(void) {
#ifndef _CAVE_
    wglMakeCurrent(hdc, 0); // Remove the rendering context from our device context
    wglDeleteContext(hrc); // Delete our rendering context

    ReleaseDC(hwnd, hdc); // Release the device context from our window
#endif
}
#ifndef _CAVE_
/**
  create30Context creates an OpenGL context and attaches it to the window provided by
  the HWND. This method currently creates an OpenGL 3.2 context by default, but will default
  to an OpenGL 2.1 capable context if the OpenGL 3.2 context cannot be created.
*/
bool OpenGLContext::create30Context(HWND hwnd) {
  this->hwnd = hwnd; // Set the HWND for our window

  hdc = GetDC(hwnd); // Get the device context for our window

  PIXELFORMATDESCRIPTOR pfd; // Create a new PIXELFORMATDESCRIPTOR (PFD)
  memset(&pfd, 0, sizeof(PIXELFORMATDESCRIPTOR)); // Clear our  PFD
  pfd.nSize = sizeof(PIXELFORMATDESCRIPTOR); // Set the size of the PFD to the size of the class
  pfd.dwFlags = PFD_DOUBLEBUFFER | PFD_SUPPORT_OPENGL | PFD_DRAW_TO_WINDOW; // Enable double buffering, opengl support and drawing to a window
  pfd.iPixelType = PFD_TYPE_RGBA; // Set our application to use RGBA pixels
  pfd.cColorBits = 32; // Give us 32 bits of color information (the higher, the more colors)
  pfd.cDepthBits = 32; // Give us 32 bits of depth information (the higher, the more depth levels)
  pfd.iLayerType = PFD_MAIN_PLANE; // Set the layer of the PFD

  int nPixelFormat = ChoosePixelFormat(hdc, &pfd); // Check if our PFD is valid and get a pixel format back
  if (nPixelFormat == 0) // If it fails
      return false;

  bool bResult = SetPixelFormat(hdc, nPixelFormat, &pfd); // Try and set the pixel format based on our PFD
  if (!bResult) // If it fails
    return false;

  HGLRC tempOpenGLContext = wglCreateContext(hdc); // Create an OpenGL 2.1 context for our device context
  wglMakeCurrent(hdc, tempOpenGLContext); // Make the OpenGL 2.1 context current and active

  GLenum error = glewInit(); // Enable GLEW
  if (error != GLEW_OK) // If GLEW fails
    return false;

  int attributes[] = {
    WGL_CONTEXT_MAJOR_VERSION_ARB, 3, // Set the MAJOR version of OpenGL to 3
    WGL_CONTEXT_MINOR_VERSION_ARB, 2, // Set the MINOR version of OpenGL to 2
    WGL_CONTEXT_FLAGS_ARB, WGL_CONTEXT_FORWARD_COMPATIBLE_BIT_ARB, // Set our OpenGL context to be forward compatible
    0
  };

  if (wglewIsSupported("WGL_ARB_create_context") == 1) { // If the OpenGL 3.x context creation extension is available
    hrc = wglCreateContextAttribsARB(hdc, NULL, attributes); // Create and OpenGL 3.x context based on the given attributes
    wglMakeCurrent(NULL, NULL); // Remove the temporary context from being active
    wglDeleteContext(tempOpenGLContext); // Delete the temporary OpenGL 2.1 context
    wglMakeCurrent(hdc, hrc); // Make our OpenGL 3.0 context current
  }
  else {
    hrc = tempOpenGLContext; // If we didn't have support for OpenGL 3.x and up, use the OpenGL 2.1 context
  }

  int glVersion[2] = {-1, -1}; // Set some default values for the version
  glGetIntegerv(GL_MAJOR_VERSION, &glVersion[0]); // Get back the OpenGL MAJOR version we are using
  glGetIntegerv(GL_MINOR_VERSION, &glVersion[1]); // Get back the OpenGL MAJOR version we are using

  std::cout << "Using OpenGL: " << glVersion[0] << "." << glVersion[1] << std::endl; // Output which version of OpenGL we are using

  return true; // We have successfully created a context, return true
}
#endif



//Globals
GLuint programID;
GLuint MatrixID;
GLuint MatrixID2;
GLuint Texture;
GLuint TextureID ;


GLuint vertexbuffer;
GLuint uvbuffer;




GLuint LoadShaders(const char * vertex_file_path,const char * fragment_file_path){

  // Create the shaders
  GLuint VertexShaderID = glCreateShader(GL_VERTEX_SHADER);
  GLuint FragmentShaderID = glCreateShader(GL_FRAGMENT_SHADER);

  // Read the Vertex Shader code from the file
  std::string VertexShaderCode;
  std::ifstream VertexShaderStream(vertex_file_path, std::ios::in);
  if(VertexShaderStream.is_open()){
    std::string Line = "";
    while(getline(VertexShaderStream, Line))
      VertexShaderCode += "\n" + Line;
    VertexShaderStream.close();
  }

  // Read the Fragment Shader code from the file
  std::string FragmentShaderCode;
  std::ifstream FragmentShaderStream(fragment_file_path, std::ios::in);
  if(FragmentShaderStream.is_open()){
    std::string Line = "";
    while(getline(FragmentShaderStream, Line))
      FragmentShaderCode += "\n" + Line;
    FragmentShaderStream.close();
  }



  GLint Result = GL_FALSE;
  int InfoLogLength;



  // Compile Vertex Shader
  printf("Compiling shader : %s\n", vertex_file_path);
  char const * VertexSourcePointer = VertexShaderCode.c_str();
  glShaderSource(VertexShaderID, 1, &VertexSourcePointer , NULL);
  glCompileShader(VertexShaderID);

  // Check Vertex Shader
  glGetShaderiv(VertexShaderID, GL_COMPILE_STATUS, &Result);
  glGetShaderiv(VertexShaderID, GL_INFO_LOG_LENGTH, &InfoLogLength);
  std::vector<char> VertexShaderErrorMessage(InfoLogLength);
  glGetShaderInfoLog(VertexShaderID, InfoLogLength, NULL, &VertexShaderErrorMessage[0]);
  fprintf(stdout, "%s\n", &VertexShaderErrorMessage[0]);




  // Compile Fragment Shader
  printf("Compiling shader : %s\n", fragment_file_path);
  char const * FragmentSourcePointer = FragmentShaderCode.c_str();
  glShaderSource(FragmentShaderID, 1, &FragmentSourcePointer , NULL);
  glCompileShader(FragmentShaderID);

  // Check Fragment Shader
  glGetShaderiv(FragmentShaderID, GL_COMPILE_STATUS, &Result);
  glGetShaderiv(FragmentShaderID, GL_INFO_LOG_LENGTH, &InfoLogLength);
  std::vector<char> FragmentShaderErrorMessage(InfoLogLength);
  glGetShaderInfoLog(FragmentShaderID, InfoLogLength, NULL, &FragmentShaderErrorMessage[0]);
  fprintf(stdout, "%s\n", &FragmentShaderErrorMessage[0]);
  

  // Link the program
  fprintf(stdout, "Linking program\n");
  GLuint ProgramID = glCreateProgram();
  glAttachShader(ProgramID, VertexShaderID);
  glAttachShader(ProgramID, FragmentShaderID);
  glLinkProgram(ProgramID);

  // Check the program
  glGetProgramiv(ProgramID, GL_LINK_STATUS, &Result);
  glGetProgramiv(ProgramID, GL_INFO_LOG_LENGTH, &InfoLogLength);
  std::vector<char> ProgramErrorMessage( InfoLogLength );
  glGetProgramInfoLog(ProgramID, InfoLogLength, NULL, &ProgramErrorMessage[0]);
  fprintf(stdout, "%s\n", &ProgramErrorMessage[0]);
  
  glDeleteShader(VertexShaderID);
  glDeleteShader(FragmentShaderID);

  return ProgramID;
}


GLuint loadBMP_custom(const char * imagepath){

  printf("Reading image %s\n", imagepath);

  // Data read from the header of the BMP file
  unsigned char header[54];
  unsigned int dataPos;
  unsigned int imageSize;
  unsigned int width, height;
  // Actual RGB data
  unsigned char * data;

  // Open the file
  FILE * file = fopen(imagepath,"rb");
  if (!file)							    {printf("Image could not be opened\n"); return 0;}

  // Read the header, i.e. the 54 first bytes

  // If less than 54 byes are read, problem
  if ( fread(header, 1, 54, file)!=54 ){ 
    printf("Not a correct BMP file\n");
    return false;
  }
  // A BMP files always begins with "BM"
  if ( header[0]!='B' || header[1]!='M' ){
    printf("Not a correct BMP file\n");
    return 0;
  }
  // Make sure this is a 24bpp file
  if ( *(int*)&(header[0x1E])!=0  )         {printf("Not a correct BMP file\n");    return 0;}
  if ( *(int*)&(header[0x1C])!=24 )         {printf("Not a correct BMP file\n");    return 0;}

  // Read the information about the image
  dataPos    = *(int*)&(header[0x0A]);
  imageSize  = *(int*)&(header[0x22]);
  width      = *(int*)&(header[0x12]);
  height     = *(int*)&(header[0x16]);

  // Some BMP files are misformatted, guess missing information
  if (imageSize==0)    imageSize=width*height*3; // 3 : one byte for each Red, Green and Blue component
  if (dataPos==0)      dataPos=54; // The BMP header is done that way

  // Create a buffer
  data = new unsigned char [imageSize];

  // Read the actual data from the file into the buffer
  fread(data,1,imageSize,file);

  // Everything is in memory now, the file wan be closed
  fclose (file);

  // Create one OpenGL texture
  GLuint textureID;
  glGenTextures(1, &textureID);
  
  // "Bind" the newly created texture : all future texture functions will modify this texture
  glBindTexture(GL_TEXTURE_2D, textureID);

  // Give the image to OpenGL
  glTexImage2D(GL_TEXTURE_2D, 0,GL_RGB, width, height, 0, GL_BGR, GL_UNSIGNED_BYTE, data);

  // Poor filtering, or ...
  //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
  //glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST); 

  // ... nice trilinear filtering.
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_REPEAT);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR_MIPMAP_LINEAR); 
  glGenerateMipmap(GL_TEXTURE_2D);

  // Return the ID of the texture we just created
  return textureID;
}








#define PEONS_AREA_WIDTH 30
//#define HALF_SIZE_OF_FIELD 30
#define KERNEL_HALF_SIZE 5
#define MIN_PEON_DIST 1.5
#define STEP_PEON_DIST 4
#define FIELD_OF_VIEW 100



//vector<peon> peons;
vector<peon*> v_peons[PEONS_AREA_WIDTH][PEONS_AREA_WIDTH];
vector<peon> a_peons;

float distance(float x1, float y1, float x2, float y2) {
  return sqrt( (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) );
}

void posToCoords(const float& x,const float& z, int& coordX, int& coordY){
    coordX = (int)(x);
    coordY = (int)(z);
}

bool crossingDet(peon& p_base, peon& p_detect, float& crossX, float& crossY){

  float bx1 = sin( p_base.angle);
  float by1 = cos( p_base.angle);
  float bx2 = sin( p_detect.angle);
  float by2 = cos( p_detect.angle);
  float bx3 = (p_detect.x - p_base.x);
  float by3 = (p_detect.z - p_base.z);
  
    
   float rat = -by3*bx2 + bx3*by2 /-by1*bx2 + bx1*by2;
   
   crossX = p_base.x + rat * bx1;
   crossY = p_base.z + rat * by1;

   if ( crossX >= 0 && crossY >= 0 ) return true; // blbost

   return false;
}

void detectCollision(peon& p_base, peon& p_detect){
    
        
    float dist = distance( p_base.x, p_base.z, p_detect.x, p_detect.z );

    if( dist < MIN_PEON_DIST ){
        p_base.realSpeed = 0.0f;		
        return;
    }

    glm::vec2 vec_detect = normalize( glm::vec2(p_detect.x, p_detect.z) - glm::vec2(p_base.x, p_base.z) );
    float angle_detect;
    if ( vec_detect.y >= 0 )
      angle_detect = atan2( vec_detect.x, vec_detect.y ); 
    else
      angle_detect = 3.141592 - atan2( vec_detect.x, vec_detect.y ); 
    
    // the detected peon is in the field of view
    if(abs(angle_detect - p_base.angle) <= FIELD_OF_VIEW/2 ){
        if( dist < MIN_PEON_DIST + STEP_PEON_DIST *1 ) {
             //p_base.realSpeed = 0.0f;
            float crosX, crosY;
            if ( crossingDet( p_base, p_detect, crosX, crosY) ) { // prusecik primek
                float distanceCrossPoint1 = distance( p_base.x, p_base.z, crosX, crosY );
                float distanceCrossPoint2 = distance( p_detect.x, p_detect.z, crosX, crosY );
                float timeToCross1 = distanceCrossPoint1 / p_base.realSpeed;
                float timeToCross2 = distanceCrossPoint2 / p_detect.realSpeed;
                float dif = 1.5f / p_detect.realSpeed;

                //TODO: omezeni na polop��mky
                //TODO: Volba �hlu
                

                if ( timeToCross1 >= timeToCross2 - dif && timeToCross1 <= timeToCross2) {
                    //crash
           //         p_base.realSpeed += 0.01;
                    p_base.angle -= 0.1;
            //		a_peons[0].x = crosX; a_peons[0].y = 0.0f; a_peons[0].realSpeed = 0.0f; a_peons[0].z = crosY;
                }
                if ( timeToCross1 <= timeToCross2 + dif && timeToCross1 > timeToCross2) {
            //        p_base.realSpeed -= 0.01;
                    p_base.angle -= 0.1;
            //		a_peons[0].x = crosX; a_peons[0].y = 0.0f; a_peons[0].realSpeed = 0.0f; a_peons[0].z = crosY;
                }

            } else {
            
                //TODO Vzdalenost bodu od p��mky a logika

            }



        }
    }

} 

void OpenGLContext::generatePeon() {
    int pos; 

    for (int i = 1; i < PEONS_AREA_WIDTH; i+=5) {
      for (int j = 1; j < PEONS_AREA_WIDTH; j+=5) {
        peon p;
        p.x = i;
        p.y = 0.0;
        p.z = j;
        p.angle = (float(rand()) / 32767.0f) * 6.28f;
        //p.angle = 5.0f;
        p.direction = p.angle;
        p.speed = 0.06f;
        p.realSpeed = 0.01f; 
        a_peons.push_back( p );
        v_peons[i][j].push_back( &a_peons[a_peons.size()-1] );
      }
    }
}



void OpenGLContext::updatePeon() {
 
  
  int coordX, coordY;
  int coordXNew, coordYNew;

  //each peon
  for(vector<peon>::iterator it = a_peons.begin(); it!=a_peons.end(); ++it){

      //borders
      if ((*it).x > PEONS_AREA_WIDTH - 1) { (*it).angle = (*it).angle -3.14f; (*it).direction = (*it).angle; }
      if ((*it).x <= 0){ (*it).angle = (*it).angle -3.14f; (*it).direction = (*it).angle;}
      if ((*it).z > PEONS_AREA_WIDTH - 1) { (*it).angle = 3.14f - (*it).angle; (*it).direction = (*it).angle;}
      if ((*it).z <= 0){ (*it).angle = 3.14f - (*it).angle; (*it).direction = (*it).angle;}
     

      posToCoords((*it).x, (*it).z, coordX, coordY);
      //neighbourArea
      for(int s = -KERNEL_HALF_SIZE; s<=KERNEL_HALF_SIZE; s++){
         for(int t = -KERNEL_HALF_SIZE; t<=KERNEL_HALF_SIZE; t++){
            int coordX_dct = coordX + s; 
            int coordY_dct = coordY + t;
            if( coordX_dct < 0 || coordY_dct < 0 || coordX_dct > PEONS_AREA_WIDTH-1 || coordY_dct > PEONS_AREA_WIDTH-1 ) continue;


            //each detected peon in the cell [x][y]
             for(vector<peon*>::iterator it_detect = v_peons[coordX_dct][coordY_dct].begin();
                 it_detect != v_peons[coordX_dct][coordY_dct].end(); ++it_detect){
                     // detect collision with itself
                     if ( &*it == &**it_detect ) continue;
                     detectCollision( *it, **it_detect ); 
                                         
             }
         }
      }
   
      (*it).x += sin((*it).angle) * (*it).realSpeed;    
      (*it).z += cos((*it).angle) * (*it).realSpeed;
    
      posToCoords((*it).x, (*it).z, coordXNew, coordYNew);
      if ( coordXNew != coordX || coordYNew != coordY) {
         for ( int i = 0; i < v_peons[coordX][coordY].size(); i++) {
              if ( &*it == &*v_peons[coordX][coordY][i] ) {
              
              }
         }	     

         v_peons[coordXNew][coordYNew].push_back( &*it );
      }


  }
//  for (int i = 0; i < PEONS_AREA_WIDTH; i++) {
//	  for(int j = 0; j < PEONS_AREA_WIDTH; j++) {
//	    
//		/*if (v_peons[i][j].x > soF) peons[i][j].x = -soF;
//		if (peons[i][j].x < -soF) peons[i][j].x = soF;
//		if (peons[i][j].z > soF){ peons[i][j].angle = 3.14f - peons[i][j].angle; peons[i][j].direction = peons[i][j].angle;}
//		if (peons[i][j].z < -soF){ peons[i][j].angle = 3.14f - peons[i][j].angle; peons[i][j].direction = peons[i][j].angle;}
//		*/
//		canMove = true; //can make standard step
//	  
//	  //Collision detection
//	  
//	  }
//  } 
//	
///*	for (int j = 0; j < signed(peons.size()); j++) {
//	  if (i == j) continue; //dont get collision with itself
//
//	  float d = distance(peons[i][j].x, peons[i][j].z, peons[j].x, peons[j].z); 
//	  if (d < 2.0f) {
//		  canMove = false;
//		 }
//
//		 
//	}*/     
     
    //if (canMove) {
    //  peons[i][j].x += sin(peons[i][j].angle) * peons[i][j].speed;    
    //  peons[i][j].z += cos(peons[i][j].angle) * peons[i][j].speed;
    //}

 }
  




/*
void OpenGLContext::updatePeon() {

   for (int i = 0; i < signed(peons.size()); i++) {


    
      if (peons[i].x > soF) peons[i].x = -soF;
      if (peons[i].x < -soF) peons[i].x = soF;
  //    if (peons[i].z > soF) peons[i].z = -soF;
  //    if (peons[i].z < -soF) peons[i].z = soF;
    
   //   if (peons[i].x > soF) { peons[i].a = peons[i].a -3.14f; peons[i].direction = peons[i].a; }
   //   if (peons[i].x < -soF){ peons[i].a = peons[i].a -3.14f; peons[i].direction = peons[i].a;}
      if (peons[i].z > soF){ peons[i].a = 3.14f - peons[i].a; peons[i].direction = peons[i].a;}
      if (peons[i].z < -soF){ peons[i].a = 3.14f - peons[i].a; peons[i].direction = peons[i].a;}
     
      if (peons[i].stop <= 0) { 
        peons[i].x += sin(peons[i].a) * peons[i].s;    
        peons[i].z += cos(peons[i].a) * peons[i].s;
      } else {
        peons[i].stop -= 1;        
      }

      peons[i].stopR -= 1;
     
      peons[i].a -= (peons[i].a - peons[i].direction) / 100.0f;

      //collision with other
      for (int j = 0; j < signed(peons.size()); j++) {
        if (i != j) {
          float d = distance(peons[i].x, peons[i].z, peons[j].x, peons[j].z); 
          if (d < 2.0f) {
            peons[i].colCount++; 
            if (peons[i].colCount > 0 && peons[i].stopR <= 0) {
           
              int buf = peons[i].a;
              peons[i].a = peons[j].a + 0.3f;//asin((peons[j].z - peons[i].z) / d) ;//(float(rand()) / 32767.0f) * 6.28f;
              peons[j].a = buf + 0.3f;//acos((peons[j].z - peons[i].z) / d) ;//(float(rand()) / 32767.0f) * 6.28f;
              peons[i].colCount = 0;
              peons[j].colCount = 0;
              peons[i].stop = 10;
              peons[j].stop = 10;
              peons[i].stopR = 50;
              peons[j].stopR = 50;

     /*         peons[i].x += sin(peons[i].a) * peons[i].s;    
              peons[i].z += cos(peons[i].a) * peons[i].s;

              peons[j].x += sin(peons[j].a) * peons[j].s;    
              peons[j].z += cos(peons[j].a) * peons[j].s;   
              
            
             }            
           }
         }
       }


      //collison with enviroment
      /* CENTER OUT*/
      /*
       float d = distance(peons[i].x, peons[i].z, 0.0, 0.0); 
       if (d < 20.0f) {
         peons[i].direction = 3.14f - atan( peons[i].z / peons[i].x );
       }
/*
       // CENTER in
       float x1 = 0.0f;
       float z1 = 0.0f;
       float d = distance(peons[i].x, peons[i].z, x1, z1); 
       if (d < 50.0f) {
         if (peons[i].z > 0)
           peons[i].direction = 3.14 + atan( (x1 - peons[i].x) / (z1 - peons[i].z) );
         else 
           peons[i].direction = 6.28 + atan( (x1 - peons[i].x) / (z1 - peons[i].z) );
       }
       if (d < 5.0f) {
         peons[i].stop = 10;
       }
       

   }
            
}*/

void OpenGLContext::setupScene(void) {
  
  glClearColor(0.0f, 0.0f, 0.0f, 0.0f); // Set the clear color based on Microsofts CornflowerBlue (default in XNA)
 
  // Ensure we can capture the escape key being pressed below
 // glfwEnable( GLFW_STICKY_KEYS );
 // glfwSetMousePos(1024/2, 768/2);

  // Dark blue background
  glClearColor(0.0f, 0.0f, 0.3f, 0.0f);


    glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
    glEnable(GL_LIGHTING);
    glEnable(GL_LIGHT0);
    glEnable(GL_DEPTH_TEST);

    GLfloat light_amb[4] = {0.4f, 0.4f, 0.4f, 1.0f};
    GLfloat light_dif[4] = {0.0f, 0.0f, 0.0f, 1.0f};
    GLfloat light_spe[4] = {0.5f, 0.5f, 0.5f, 1.0f};
    GLfloat light_pos[4] = {0.0f, 10.0f, 5.0f, 1.0f};
    GLfloat light_dir[4] = {0.0f, 0.0f, 0.0f, 1.0f};
  
    GLfloat material_amd[4] = {1.0f, 0.1f, 0.1f, 1.0f};
    GLfloat material_spe[4] = {0.8f, 0.8f, 0.8f, 1.0f};

    glMaterialfv(GL_FRONT_AND_BACK,  GL_AMBIENT, material_amd);
    glMaterialfv(GL_FRONT_AND_BACK,  GL_DIFFUSE, material_amd);
    glMaterialfv(GL_FRONT_AND_BACK, GL_SPECULAR, material_spe);
    glMaterialf(GL_FRONT_AND_BACK, GL_SHININESS, 128);

    glEnable(GL_LIGHT1);
    glLightfv(GL_LIGHT1, GL_POSITION      , light_pos);
    glLightfv(GL_LIGHT1, GL_SPOT_DIRECTION, light_dir);
    glLightfv(GL_LIGHT1, GL_AMBIENT       , light_amb);
    glLightfv(GL_LIGHT1, GL_DIFFUSE       , light_dif);
    glLightfv(GL_LIGHT1, GL_SPECULAR	     , light_spe);
    glLightf (GL_LIGHT1, GL_SPOT_CUTOFF	 , 24);
    glLightf (GL_LIGHT1, GL_SPOT_EXPONENT , 128);



  // Enable depth test
//  glEnable(GL_DEPTH_TEST);
  // Accept fragment if it closer to the camera than the former one
  glDepthFunc(GL_LESS); 

  // Cull triangles which normal is not towards the camera
  glEnable(GL_CULL_FACE);
  std::cerr << "setupScene 0\n";
  GLuint VertexArrayID;
  glGenVertexArrays(1, &VertexArrayID);
  glBindVertexArray(VertexArrayID);
  std::cerr << "setupScene 1\n";
  // Create and compile our GLSL program from the shaders
  programID = LoadShaders( "TransformVertexShader.vertexshader", "TextureFragmentShader.fragmentshader" );
  std::cerr << "setupScene 2\n";
  // Get a handle for our "MVP" uniform
  MatrixID = glGetUniformLocation(programID, "u_MV");
  MatrixID2 = glGetUniformLocation(programID, "u_P");
  std::cerr << "setupScene 3\n";
  // Load the texture
  Texture = loadBMP_custom("Models/legoMan/Texture.bmp"); //loadDDS("uvmap.DDS");
  std::cerr << "setupScene 4\n";
  // Get a handle for our "myTextureSampler" uniform
  TextureID  = glGetUniformLocation(programID, "myTextureSampler");
  std::cerr << "setupScene 5\n";
  // Read our .obj file
  //bool res = loadOBJ("Models/legoMan/LegoMan.obj", vertices, uvs, normals);

  // Load it into a VBO  	
  glGenBuffers(1, &vertexbuffer);
  glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer);
  glBufferData(GL_ARRAY_BUFFER, LegoManNumVerts * sizeof(float)*3, LegoManVerts, GL_STATIC_DRAW);
  std::cerr << "setupScene 6\n";
  glGenBuffers(1, &uvbuffer);
  glBindBuffer(GL_ARRAY_BUFFER, uvbuffer);
  glBufferData(GL_ARRAY_BUFFER, LegoManNumVerts * sizeof(float)*2, LegoManTexCoords, GL_STATIC_DRAW);
  std::cerr << "setupScene 7\n";
}

#ifndef _CAVE_
void OpenGLContext::reshapeWindow(int w, int h) {
  windowWidth = w; // Set the window width
  windowHeight = h; // Set the window height
   
}
#endif



glm::mat4 OpenGLContext::getViewMatrix() const {
  return ViewMatrix;
}
glm::mat4 OpenGLContext::getProjectionMatrix() const{
  return ProjectionMatrix;
}

void OpenGLContext::setViewMatrix(const glm::mat4& mat)
{
  ViewMatrix = mat;
}
void OpenGLContext::setProjectionMatrix(const glm::mat4& mat)
{
  ProjectionMatrix = mat;
}

// Initial position : on +Z
glm::vec3 position = glm::vec3( 5.0f, 30.0f, 5.0f ); 
// Initial horizontal angle : toward -Z
float horizontalAngle = -5.14f;
// Initial vertical angle : none
float verticalAngle = -1.5f;
// Initial Field of View
float initialFoV = 55.0f;

float speed = 0.01f;
float angleSpeed = 0.001f;

#ifndef _CAVE_
void computeMatricesFromInputs(){
  // glfwGetTime is called only once, the first time this function is called
  static double lastTime = clock();

  // Compute time difference between current and last frame
  double currentTime = clock();
  float deltaTime = float(currentTime - lastTime);

  // Get mouse position
  int xpos, ypos;
 //TD glfwGetMousePos(&xpos, &ypos);

  // Reset mouse position for next frame
 //TD glfwSetMousePos(1024/2, 768/2);

  // Compute new orientation
//TD  horizontalAngle += mouseSpeed * float(1024/2 - xpos );
 //TD verticalAngle   += mouseSpeed * float( 768/2 - ypos );

  // Direction : Spherical coordinates to Cartesian coordinates conversion
  glm::vec3 direction(
    cos(verticalAngle) * sin(horizontalAngle), 
    sin(verticalAngle),
    cos(verticalAngle) * cos(horizontalAngle)
  );
  
  // Right vector
  glm::vec3 right = glm::vec3(
    sin(horizontalAngle - 3.14f/2.0f), 
    0,
    cos(horizontalAngle - 3.14f/2.0f)
  );
  
  // Up vector
  glm::vec3 up = glm::cross( right, direction );
  
  if (pkeys['W'] == true )
  {
     position += direction * deltaTime * speed;
  }

  if (pkeys['S'] == true )
  {
    position -= direction * deltaTime * speed;
  }

  if (pkeys['D'] == true )
  {
    position += right * deltaTime * speed;
  }

  if (pkeys['A'] == true )
  {
    position -= right * deltaTime * speed;
  }

  if (pkeys[VK_UP] == true )
  {
    verticalAngle += angleSpeed;
  }

  if (pkeys[VK_DOWN] == true )
  {
    verticalAngle -= angleSpeed;
  }

  if (pkeys[VK_LEFT] == true )
  {
    horizontalAngle += angleSpeed;
  }

  if (pkeys[VK_RIGHT] == true )
  {
    horizontalAngle -= angleSpeed;
  }
  
  float FoV = initialFoV;// * glfwGetMouseWheel();

  // Projection matrix : 45� Field of View, 4:3 ratio, display range : 0.1 unit <-> 100 units
  ProjectionMatrix = glm::perspective(FoV, 4.0f / 3.0f, 0.1f, 5000.0f);
  // Camera matrix
  ViewMatrix       = glm::lookAt(
                position,           // Camera is here
                position+direction, // and looks here : at the same position, plus "direction"
                up                  // Head is up (set to 0,-1,0 to look upside-down)
               );

  // For the next frame, the "last time" will be "now"
  lastTime = currentTime;
}
#endif

void OpenGLContext::drawPeon(peon& p) const {
  
   // Bind our texture in Texture Unit 0
    glActiveTexture(GL_TEXTURE0);
    glBindTexture(GL_TEXTURE_2D, Texture);
    // Set our "myTextureSampler" sampler to user Texture Unit 0
    glUniform1i(TextureID, 0);
    
    // Compute the MVP matrix from keyboard and mouse input
#ifndef _CAVE_
    computeMatricesFromInputs();
#endif
    glm::mat4 ProjectionMatrix = getProjectionMatrix();
    glm::mat4 ViewMatrix = getViewMatrix();
    glm::mat4 ModelMatrix = glm::mat4(1.0,0.0,0.0,0.0, 0.0,1.0,0.0,0.0, 0.0,0.0,1.0,0.0, p.x,p.y,p.z,1.0) * 
                            glm::mat4(cos(p.angle),0.0,-sin(p.angle),0.0, 0.0,1.0,0.0,0.0, sin(p.angle),0.0,cos(p.angle),0.0, 0.0,0.0,0.0,1.0);
    glm::mat4 MV = ViewMatrix * ModelMatrix;
   // glm::mat4 MVP = ProjectionMatrix * ViewMatrix * ModelMatrix;

    // Send our transformation to the currently bound shader, 
    // in the "MVP" uniform
    glUniformMatrix4fv(MatrixID, 1, GL_FALSE, &MV[0][0]);
    glUniformMatrix4fv(MatrixID2, 1, GL_FALSE, &ProjectionMatrix[0][0]);


    // 1rst attribute buffer : vertices
    glEnableVertexAttribArray(0);
    glBindBuffer(GL_ARRAY_BUFFER, vertexbuffer);
    glVertexAttribPointer(
      0,                  // attribute
      3,                  // size
      GL_FLOAT,           // type
      GL_FALSE,           // normalized?
      0,                  // stride
      (void*)0            // array buffer offset
    );

    // 2nd attribute buffer : UVs
    glEnableVertexAttribArray(1);
    glBindBuffer(GL_ARRAY_BUFFER, uvbuffer);
    glVertexAttribPointer(
      1,                                // attribute
      2,                                // size
      GL_FLOAT,                         // type
      GL_FALSE,                         // normalized?
      0,                                // stride
      (void*)0                          // array buffer offset
    );

    // Draw the triangle !
    glDrawArrays(GL_TRIANGLES, 0, LegoManNumVerts );
   
    glDisableVertexAttribArray(0);
    glDisableVertexAttribArray(1);

}

void OpenGLContext::renderScene(void) {
    // Clear the screen
    glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    // Use our shader
    glUseProgram(programID);
    
    for (int i = 0; i < signed(a_peons.size()); i++) {
       drawPeon(a_peons[i]);
    }
#ifndef _CAVE_
  SwapBuffers(hdc); // Swap buffers so we can see our rendering
#endif
}
#ifndef _CAVE_
void OpenGLContext::updateKeyboard(bool* key){
  pkeys = key;
}
#endif
